import numpy as np
from cv2 import cv2
import re
import matplotlib.pyplot as plt
from numpy.core.defchararray import count
from scipy.stats.stats import zscore
from utils2 import *
import random
from sklearn.cluster import KMeans
from sklearn import decomposition
import matplotlib
import matplotlib.image as gImage
from sklearn.manifold import TSNE
from matplotlib.ticker import FuncFormatter
import scipy.stats
import time
import random
from sklearn.metrics import confusion_matrix
import copy
import pickle
from sklearn.cluster import KMeans
from sklearn.cluster import DBSCAN
from sklearn.cluster import AgglomerativeClustering
from sklearn_extra.cluster import KMedoids
from sklearn import metrics
from sklearn.metrics import pairwise_distances



feature_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2_method2.npy'
anchorImg_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorImgs_method2'
referenceLabel_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorReferenceLabel_method2.txt' # 默认没有reference label
position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2_sync_position.npy' # 经纬度

pkuBirdViewImg = 'D:\\Research\\2020ContrastiveLearningForSceneLabel\\Data\\pkuBirdView.png'


feat = np.load(feature_path)
frame_count = np.shape(feat)[0]

sampleNum = 16
anchor_pos_list, anchor_neg_list = getAnchorPosNegIdx3(anchorImg_path, sampleNum = sampleNum, sample_interval = 2)
anchor_idx = [anchor_pos_list[i][0] for i in range(len(anchor_pos_list))]
training_idx = []
for i in anchor_pos_list:
    training_idx += i[1]

if referenceLabel_path != '':
    f = open(referenceLabel_path,'r')
    lines = f.readlines()
    rf_label_anchor = [int(i[0]) for i in lines]
    rf_label_allFrame = list(range(frame_count))
    for i, idx in enumerate(anchor_idx):
        if i == len(anchor_idx) - 1:
            break
        for j in range(anchor_idx[i], anchor_idx[i+1]):
            rf_label_allFrame[j] = rf_label_anchor[i]

    for i in range(anchor_idx[-1], frame_count):
        rf_label_allFrame[i] = rf_label_anchor[-1]
    for i in range(0, anchor_idx[0]):
        rf_label_allFrame[i] = rf_label_anchor[-1]

else:
    rf_label_anchor = [0 for i in anchor_idx]
    rf_label_allFrame = [0 for i in range(frame_count)]

rf_label_training = [rf_label_anchor[i] for i in range(len(rf_label_anchor)) for j in range(sampleNum)]

rf_label_anchor = np.array(rf_label_anchor)
rf_label_training = np.array(rf_label_training)
rf_label_allFrame = np.array(rf_label_allFrame)

training_feat = feat[training_idx]


rf_label_training[rf_label_training == 4] = 1
rf_label_training[rf_label_training == 5] = 1

# ======================数据加载结束======================

n = 15

neighbor_similarity = [np.sum(feat[i] * feat[i+n]) for i in range(np.shape(feat)[0] - n)]
for i in range(n):
    neighbor_similarity.append(1)
neighbor_similarity = np.array(neighbor_similarity)

plt.figure()
plt.plot(neighbor_similarity)
plt.title('Neighbor similarity')

# ===== 把相邻点的相似度画在轨迹上=====
GNSS = np.load(position_path)
rotate = 0 # 角度制
shiftX = 625
shiftY = 620
dx = 0.777
dy = 0.777 # 以上参数都是手调的
alpha = 0.5

GNSS[:,0] = -GNSS[:,0]
GNSS[:,1] *= dx
GNSS[:,0] *= dy
GNSS[:,1] += shiftX
GNSS[:,0] += shiftY


fig = plt.figure()
ax = fig.add_subplot(111)
img = gImage.imread(pkuBirdViewImg)
img = img[0:1087,:,:]
img[:,:,3] = alpha
ax.imshow(img, zorder = 0)

cmap = 'bwr'

thresh = 0.5

ax.plot(GNSS[:,1], GNSS[:,0], zorder = 0)

ax.scatter(GNSS[neighbor_similarity < thresh, 1], GNSS[neighbor_similarity < thresh, 0], c = 'r', s = 15, zorder = 1)
plt.scatter(GNSS[anchor_idx,1], GNSS[anchor_idx,0], s = 10, c='k', zorder = 3) # 所有锚点
ax.scatter(GNSS[training_idx,1], GNSS[training_idx,0], s=10, c = 'b', cmap=cmap, zorder = 1) # 车辆行驶轨迹

plt.legend(['Traj', 'Changed', 'Anchor','Training data'])




plt.figure()
plt.hist(neighbor_similarity,bins=20)


plt.show()
exit(0)